#include "StepMotor.h"
#include "math.h"

/*
    PB4 EN
    PB5 DIR
    PB6 STEP TIM_CH1
*/

StepMotor_Struct StepMotor_1={
  .step=0,
  .step_flag=0,
  .dir=0,
  .num=1,
  .en=0,
  .state=STOP
};

static void GPIOInit(void)
{
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
  GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);

  GPIOB->BRR|=GPIO_Pin_5|GPIO_Pin_6;
  GPIOB->BSRR|=GPIO_Pin_4;

  GPIO_InitTypeDef user_struct;
  user_struct.GPIO_Mode=GPIO_Mode_AF_PP;
  user_struct.GPIO_Pin=GPIO_Pin_6;
  user_struct.GPIO_Speed=GPIO_Speed_10MHz;
  GPIO_Init(GPIOB,&user_struct);

  user_struct.GPIO_Mode=GPIO_Mode_Out_PP;
  user_struct.GPIO_Pin=GPIO_Pin_4|GPIO_Pin_5;
  GPIO_Init(GPIOB,&user_struct);
}

static void timer_init(void)
{
  TIM_TimeBaseInitTypeDef timebaseInit_struct;
  TIM_TimeBaseStructInit(&timebaseInit_struct);
  timebaseInit_struct.TIM_Prescaler=71;
  timebaseInit_struct.TIM_CounterMode=TIM_CounterMode_Up;
  timebaseInit_struct.TIM_Period=0xffff;
  timebaseInit_struct.TIM_ClockDivision=TIM_CKD_DIV2;
  TIM_TimeBaseInit(TIM4,&timebaseInit_struct);
  TIM_ARRPreloadConfig(TIM4, ENABLE);

  TIM_OCInitTypeDef ocInit_struct;
  TIM_OCStructInit(&ocInit_struct);
  
  ocInit_struct.TIM_OCMode=TIM_OCMode_PWM1;
  ocInit_struct.TIM_OutputState=TIM_OutputState_Disable;
  ocInit_struct.TIM_OutputNState=TIM_OutputNState_Disable;
  ocInit_struct.TIM_Pulse=9;
  ocInit_struct.TIM_OCPolarity=TIM_OCPolarity_High;
  ocInit_struct.TIM_OCNPolarity=TIM_OCNPolarity_Low;
  ocInit_struct.TIM_OCIdleState=TIM_OCIdleState_Reset;
  ocInit_struct.TIM_OCNIdleState=TIM_OCNIdleState_Set;
  TIM_OC1Init(TIM4,&ocInit_struct);
  TIM_OC1PreloadConfig(TIM4,TIM_OCPreload_Enable);
  TIM_GenerateEvent(TIM4,TIM_EventSource_Update);

  TIM_ClearFlag(TIM4,TIM_FLAG_Update);
  NVIC_InitTypeDef nvic_struct;
  nvic_struct.NVIC_IRQChannel=TIM4_IRQn;
  nvic_struct.NVIC_IRQChannelCmd=ENABLE;
  nvic_struct.NVIC_IRQChannelPreemptionPriority=10;
  nvic_struct.NVIC_IRQChannelSubPriority=0;
  NVIC_Init(&nvic_struct);
  TIM_ITConfig(TIM4,TIM_IT_CC1,ENABLE);
}

void StepMotor_Init(void)
{
  GPIOInit();
  timer_init();
}

void StepMotor_Enable(StepMotor_Struct* user_struct)
{
  switch (user_struct->num)
  {
  case 1:
    GPIOB->BRR|=GPIO_Pin_4;
    user_struct->en=1;
    break;
  
  default:
    break;
  }
}

void StepMotor_Disable(StepMotor_Struct* user_struct)
{
  switch (user_struct->num)
  {
  case 1:
    GPIOB->BSRR|=GPIO_Pin_4;
    user_struct->en=0;
    break;
  
  default:
    break;
  }
}

void StepMotor_Forward(StepMotor_Struct* user_struct)
{
  switch (user_struct->num)
  {
  case 1:
    GPIOB->BSRR|=GPIO_Pin_5;
    user_struct->dir=1;
    break;
  
  default:
    break;
  }
}

void StepMotor_Backward(StepMotor_Struct* user_struct)
{
  switch (user_struct->num)
  {
  case 1:
    GPIOB->BRR|=GPIO_Pin_5;
    user_struct->dir=0;
    break;
  
  default:
    break;
  }
}

void StepMotor_Start(StepMotor_Struct* user_struct)
{
  switch (user_struct->num)
  {
  case 1:
    StepMotor_Enable(user_struct);
    TIM_Cmd(TIM4,ENABLE);
    TIM_CCxCmd(TIM4,TIM_Channel_1,TIM_CCx_Enable);
    TIM4->CNT=0;
    break;
  
  default:
    break;
  }
}

void StepMotor_Stop(StepMotor_Struct* user_struct)
{
  switch (user_struct->num)
  {
  case 1:
    StepMotor_Disable(user_struct);
    TIM_CCxCmd(TIM4,TIM_Channel_1,TIM_CCx_Disable);
    TIM_Cmd(TIM4,DISABLE);
    break;
  
  default:
    break;
  }
}

void StepMotor_RunStep(StepMotor_Struct* user_struct,long step)
{
  switch (user_struct->num)
  {
  case 1:
    if(step>=0){
      StepMotor_Forward(user_struct);
      user_struct->step=step;
    }
    else{
      StepMotor_Backward(user_struct);
      user_struct->step=-step;
    }
    StepMotor_Start(user_struct);
    user_struct->step_flag=1;
    break;
  
  default:
    break;
  }
}

void StepMotor_MoveStep(StepMotor_Struct* user_struct,int32_t step,float accel,float decel,uint32_t speed)
{
  if(step>0)
  {
    StepMotor_Forward(&StepMotor_1);
  }
  else
  {
    step=-step;
    StepMotor_Backward(&StepMotor_1);
  }
  
  user_struct->min_delay=(int32_t)(A_T/(speed));
  user_struct->step_delay=T1_FREQ_FIX*sqrt(2*ALPHA/accel);
  TIM4->ARR=user_struct->step_delay;
  TIM_GenerateEvent(TIM4,TIM_EventSource_Update);

  user_struct->max_s_lim=(speed*speed)/(2*ALPHA*accel);
  user_struct->accel_lim=(step*decel)/(accel+decel);

  if(user_struct->max_s_lim<user_struct->accel_lim)
  {
    user_struct->decel_val=-(user_struct->max_s_lim*accel/decel);
  }
  else
  {
    user_struct->decel_val=-(step-user_struct->accel_lim);
  }
  user_struct->decel_start=step+user_struct->decel_val;
  
  if(step==1)
  {
    user_struct->state=DECEL;
  }
  else
  {
    user_struct->state=ACCEL;
  }
  
	user_struct->step=step;
  user_struct->step_flag=1;
  
  StepMotor_Start(&StepMotor_1);
}

